/***
 * @Author: y.liu
 * @Date: 2023-08-31
 * @Description: 静态路径规划节点
 *  - 1. 接收地图信息和更新地图状态
 *  - 2. 封装任务路径
 *  - 3. 产生点对点的路径
 * @LastEditors: y.liu
 * @LastEditTime: 2023-08-31
 * @FilePath: global_plan_planner/src/global_path_planner.cpp
 */

#include "global_path_planner.h"
#include "common/docker_def.h"
#include "common/speed_def.h"
#include "glog_func/log.h"
#include "path_smooth/SmoothPath.h"

#include <v3_msgs/MapInfoRequest.h>
#include <cln_msgs/LogMsg.h>

#include <tf/tf.h>
#include <thread>

using namespace global_path_planner::common;

// 构造函数
GlobalPathPlanner::GlobalPathPlanner(){
    // 初始化参数
    InitParam();
    // 接收信息
    __map_Status_Sub = __nh.subscribe("/map_status_v3", 1, &GlobalPathPlanner::MapSatusCallback, this);
    __map_Grid_Sub = __nh.subscribe("/map", 1, &GlobalPathPlanner::MapGridCallback, this);
    __map_Info_Sub = __nh.subscribe("/map_info_v3", 1, &GlobalPathPlanner::MapInfoCallback, this);
    // 订阅参数变更
    __params_Update_Sub = __nh.subscribe("/robot_param_change", 1, &GlobalPathPlanner::ParamsUpdateCallback, this);
    // 默认标记点请求
    __map_Info_Client = __nh.serviceClient<v3_msgs::MapInfoRequest>("/map_info_query");
    // 生成p2p路径
    __p2p_Static_Serve = __nh.advertiseService("/p2p_path_server", &GlobalPathPlanner::getP2PPath, this);
    // 生成全覆盖路径
    __full_Cover_Path_Serve = __nh.advertiseService("/cover_path_server",&GlobalPathPlanner::getFullCoverPath, this);
    // 生成推行路径
    __implement_Path_Serve = __nh.advertiseService("/hand_path_server",&GlobalPathPlanner::getImplementPath, this);

    // 初始化路径类
    __map_Data_Ptr = std::make_shared<MapData>(MapData());
    __msg_Pub_Ptr = std::make_shared<MsgPublish>();
    __map_Proc_Ptr = std::make_shared<MapProcess>(MapProcess(__map_Data_Ptr));
    __path_Plan_Ptr = std::make_shared<::common::planning::PathPlan>();
    __full_Cover_Planner_Ptr = std::make_shared<FullCoverPlanner>(FullCoverPlanner(__robot_Radius,
                                                                                    __map_Dir,
                                                                                    __path_Width,
                                                                                    __map_Proc_Ptr,
                                                                                    __path_Plan_Ptr));
    __implement_Planner_Ptr = std::make_shared<ImplementPlanner>(ImplementPlanner());
    LOG(INFO) << "GlobalPathPlanner constructed";
}

// 析构函数
GlobalPathPlanner::~GlobalPathPlanner(){

}

// 初始化参数
void GlobalPathPlanner::InitParam(){
    // 机器人半径
    __robot_Radius = ros::param::param<float>("/global_path_planner/robot_radius", 0.45);
    LOG(INFO) << "__robot_Radius: " << __robot_Radius;
    // 地图缓存位置
    __map_Dir = ros::param::param<string>("/maps_dir", "/home/aoting/user_config/map_database/");
    LOG(INFO) << "__map_Dir: " << __map_Dir;
    // 数据缓存位置
    __cache_Dir = ros::param::param<string>("/cache_dir", "/home/aoting/user_config/cache_database/");
    LOG(INFO) << "__cache_Dir: " << __cache_Dir;
    // 调试模式
    // __debug_Mode = ros::param::param<bool>("/global_path_planner/debug_mode", false);
    // 路径宽度
    __path_Width = ros::param::param<float>("/clean_width/path_width", 0.2);
    LOG(INFO) << "__path_Width: " << __path_Width;
    // 主刷宽度
    __brush_Length = ros::param::param<float>("/clean_width/sweep_width", 0.2);
    LOG(INFO) << "__brush_Length: " << __brush_Length;
    // 充电桩的位置到滑行点距离
    __landing_Distance = ros::param::param<float>("/docker/docker_distance", 1.5);
    LOG(INFO) << "__landing_Distance: " << __landing_Distance;
    // 机器人类型
    __robotType = ros::param::param<std::string>("/robot_type_sw", "SW55A_AA");
    LOG(INFO) << "__robotType: " << __robotType;
    __map_Status_Ptr = nullptr;
    __map_Info_Ptr = nullptr;
    __map_Grid_Ptr = nullptr;
}

// 参数变更
void GlobalPathPlanner::ParamsUpdateCallback(const cln_msgs::StringArray &msg)
{
    vector<string> temp;
    temp.assign(msg.data.begin(),msg.data.end());

    bool params_updated = false;
    std::string info = "[GlobalPathPlanner::ParamsUpdateCallback]";
    for(int i = 0; i < temp.size(); i++)
    {
        if(temp[i] == "/maps_dir")
        {
            __map_Dir = ros::param::param<string>("/maps_dir", "/home/aoting/user_config/map_database/");
            params_updated = true;
            info += " __map_Dir: " + __map_Dir;
        }else if(temp[i] == "/cache_dir"){
            __cache_Dir = ros::param::param<string>("/cache_dir", "/home/aoting/user_config/cache_database/");
            params_updated = true;
            info += " __cache_Dir: " + __cache_Dir;
        }else if(temp[i] == "/clean_width"){
            __path_Width = ros::param::param<float>("/clean_width/path_width", 0.2);
            __brush_Length = ros::param::param<float>("/clean_width/sweep_width", 0.2); //主刷宽度
            params_updated = true;
            info += " __path_Width: " + std::to_string(__path_Width);
            info += " __brush_Length: " + std::to_string(__brush_Length);
            LOG(INFO) << "__path_Width from ros param: " << __path_Width;
        }
    }
    if (params_updated) {
       __msg_Pub_Ptr->SaveLogInfo("INFO", info, 0);
       LOG(INFO) << info;
    }
}

// 接收地图状态消息
void GlobalPathPlanner::MapSatusCallback(const v3_msgs::MapStatusConstPtr& msg){
    if (__map_Status_Ptr == nullptr || __map_Status_Ptr->status != msg->status) {
        std::string info = "[GlobalPathPlanner::MapSatusCallback] new map status:";
        info += std::to_string(msg->status);
        __msg_Pub_Ptr->SaveLogInfo("INFO", info, 0);
        LOG(INFO) << info;
    }
    __map_Status_Ptr = msg;
}

// 接收地图信息
void GlobalPathPlanner::MapInfoCallback(const v3_msgs::MapInfoConstPtr& msg){
    if(msg->base_info.name.empty() ||
        msg->base_info.metadata.width <= 0 ||
        msg->base_info.metadata.height <= 0) return;
    __map_Info_Ptr = msg;
    ros::Time t0 = ros::Time::now();
    __map_Data_Ptr->Reset();
    __map_Data_Ptr->ParseFuncAreas(msg); // 功能区, 当前已弃用
    __map_Data_Ptr->ParseVirtualWalls(msg);
    std::string info = "[GlobalPathPlanner::MapInfoCallback]";
    info += "map_name:" + msg->base_info.name;
    info += ", width:" + std::to_string(msg->base_info.metadata.width);
    info += ", height:" + std::to_string(msg->base_info.metadata.height);
    info += ", origin_x:" + std::to_string(msg->base_info.metadata.origin.position.x);
    info += ", origin_y:" + std::to_string(msg->base_info.metadata.origin.position.y);
    if(__map_Grid_Ptr == nullptr ||
        msg->base_info.metadata.width != __map_Grid_Ptr->info.width ||
        msg->base_info.metadata.height != __map_Grid_Ptr->info.height) return;
    // if(__map_Grid_Ptr != nullptr && __map_Info_Ptr != nullptr){
        __map_Proc_Ptr->UpdateMapImage(__map_Grid_Ptr, __path_Plan_Ptr, __robot_Radius + 0.05);
    // }
    info += ", Update map image successful! UpdateTime:" 
            + std::to_string((ros::Time::now() - t0).toSec());
    __msg_Pub_Ptr->SaveLogInfo("INFO", info, 0);
    LOG(INFO) << info;
}

// 接收栅格地图地图信息
void GlobalPathPlanner::MapGridCallback(const nav_msgs::OccupancyGridConstPtr& msg){
    cout << "MapGridCallback..." << endl;
    if(msg->data.empty() ||
        msg->info.width <= 0 ||
        msg->info.height <= 0) return;
    __map_Grid_Ptr = msg;
    ros::Time t0 = ros::Time::now();
    std::string info = "[GlobalPathPlanner::MapGridCallback]";
    info += "width:" + std::to_string(msg->info.width);
    info += ", height:" + std::to_string(msg->info.height);
    info += ", origin_x:" + std::to_string(msg->info.origin.position.x);
    info += ", origin_y:" + std::to_string(msg->info.origin.position.y);
    // if(__map_Info_Ptr == nullptr ||
    //     __map_Info_Ptr->base_info.metadata.width != msg->info.width ||
    //     __map_Info_Ptr->base_info.metadata.height != msg->info.height) return;
    // if(__map_Info_Ptr != nullptr && __map_Grid_Ptr != nullptr){
        __map_Proc_Ptr->UpdateMapImage(__map_Grid_Ptr, __path_Plan_Ptr, __robot_Radius + 0.05);
    // }
    info += ", Update map image successful! UpdateTime:" 
            + std::to_string((ros::Time::now() - t0).toSec());
    __msg_Pub_Ptr->SaveLogInfo("INFO", info, 0);
    LOG(INFO) << info;
}

// 生成全局p2p路径
bool GlobalPathPlanner::getP2PPath(v3_msgs::PathGenerateManagerRequest& req, v3_msgs::PathGenerateManagerResponse& res){
    LOG(INFO) << "getP2PPath start";
    LOG(INFO) << "option: " << (int)req.option << ", is_pile: " << (req.path_gen.is_pile ? 1 : 0)
              << ", start: (" << req.path_gen.start.position.x << ", " << req.path_gen.start.position.y
              << "), end: (" << req.path_gen.end.position.x << ", " << req.path_gen.end.position.y << ")";
    res.response.success = true;
    res.response.message = "successful";

    // 日志消息属性
    double plan_time;
    std::string info;
    int info_code = 0;
    std::string info_level = "INFO";
    bool state = true;

    ros::Time t0 = ros::Time::now();
    PoseDq p2p_path{};
    if(req.option == v3_msgs::PathGenerateManagerRequest::POSITION_EFFECTIVE){
        if(req.path_gen.is_pile){
            // 获取默认充电桩
            std::pair<bool, std::tuple<float, float, float>> docker_pose = getDefaultDockerPose();
            if(docker_pose.first){
                req.path_gen.end.position.x = std::get<0>(docker_pose.second);
                req.path_gen.end.position.y = std::get<1>(docker_pose.second);
                LOG(INFO) << "docker pose (" << req.path_gen.end.position.x << ", " << req.path_gen.end.position.y << ")";
            }
        }
        Pose check_pose(req.path_gen.end.position.x, req.path_gen.end.position.y);
        LOG(INFO) << "to check if pose(" << check_pose.X() << ", " << check_pose.Y() << ")'s effective";
        bool is_occupied = __path_Plan_Ptr->IsOccupied(check_pose);
        if(!__path_Plan_Ptr->UpdateToFree(check_pose, 0.3))
        {
            res.response.success = false;
            res.response.error_code.error_code = 0x1504;
            res.response.message = "[GlobalPathPlanner::getP2PPath]Failed to check position effective!";
            res.response.error_code.error_msg = info;
            info = "[GlobalPathPlanner::getP2PPath]" + res.response.error_code.error_msg;
            info_level = "ERROR";
            info_code = res.response.error_code.error_code;
            state = false;
        } else if (is_occupied) {
            LOG(WARNING) << "check_pose need to be updated to (" << check_pose.X() << ", " << check_pose.Y() << ")";
        }
    }else{
        uint8_t result_info = 0;
        Pose start(req.path_gen.start.position.x, req.path_gen.start.position.y);
        Pose goal(req.path_gen.end.position.x, req.path_gen.end.position.y);
        uint8_t method = start.EuclidDistance(goal) < 50.0
                             ? ::common::planning::GraphSearch::ASTAR
                             : ::common::planning::GraphSearch::JPS;
        p2p_path = __path_Plan_Ptr->SearchPathWithResult(start, 0.3, goal, 0.3, 0.05, result_info, method);
        plan_time = (ros::Time::now() - t0).toSec();
        if(p2p_path.empty()){
            auto result_string = ::common::planning::ToStringResult(result_info);
            LOG(ERROR) << "p2p failed, result_info is " << result_string;
            res.response.success = false;
            res.response.error_code.error_code = 0x1502;
            res.response.message = "[GlobalPathPlanner::getP2PPath]Failed to generate p2p path!";
            res.response.error_code.error_msg = result_string;
            info = "[GlobalPathPlanner::getP2PPath]" + res.response.error_code.error_msg;
            info_level = "ERROR";
            info_code = res.response.error_code.error_code;
            state = false;
        }else{
            using namespace global_path_planner::path_smooth;
            SmoothPoseDq(p2p_path, 0.5 * __path_Plan_Ptr->DFMap().Resolution());
            double safe_dis = sqrt(__path_Plan_Ptr->DFMap().ThreshFree()) * __path_Plan_Ptr->DFMap().Resolution();
            ProcessRiskRanges(p2p_path, __path_Plan_Ptr->DFMap(), safe_dis);
            v3_msgs::TaskPath task_path;
            geometry_msgs::Pose pose;
            for (const auto& path_point : p2p_path) {
                pose.position.x = path_point.X();
                pose.position.y = path_point.Y();
                task_path.poses.emplace_back(pose);
            }
            res.paths.emplace_back(task_path);
            info = "[GlobalPathPlanner::getP2PPath]Generate p2p path successful!";
        }

        // 调试代码
        LOG(INFO) << "to save debug p2p img";
        cv::Mat tmp_img = __map_Proc_Ptr->GetMapImg().clone();
        for (size_t i = 0; i + 1 < p2p_path.size(); ++ i) {
            cv::Point pre_p(__path_Plan_Ptr->DFMap().X2IX(p2p_path.at(i).X()), __path_Plan_Ptr->DFMap().Y2IY(p2p_path.at(i).Y()));
            cv::Point next_p(__path_Plan_Ptr->DFMap().X2IX(p2p_path.at(i+1).X()), __path_Plan_Ptr->DFMap().Y2IY(p2p_path.at(i+1).Y()));
            cv::line(tmp_img, pre_p, next_p, cv::Scalar(0));
        }
        // 起点处画圆
        cv::Point start_p(__path_Plan_Ptr->DFMap().X2IX(start.X()), __path_Plan_Ptr->DFMap().Y2IY(start.Y()));
        cv::circle(tmp_img, start_p, 3, cv::Scalar(100));
        // 目标点处画框
        cv::Point goal_p(__path_Plan_Ptr->DFMap().X2IX(goal.X()), __path_Plan_Ptr->DFMap().Y2IY(goal.Y()));
        cv::rectangle(tmp_img, cv::Point(goal_p.x - 2, goal_p.y - 2), cv::Point(goal_p.x + 2, goal_p.y + 2), cv::Scalar(100));
        std::string map_file = ::common::log::kGlobalPathPlannerLogPath + "/" + ::common::log::CurTimeString() + "_p2p_img.png";
        cv::flip(tmp_img, tmp_img, 0);
        cv::imwrite(map_file, tmp_img);
        LOG(INFO) << "save debug p2p img finish";
    }
    info = __msg_Pub_Ptr->P2PNormalLogInfo(req.path_gen.start,
                                        req.path_gen.end,
                                        plan_time, info,
                                        p2p_path.size(),
                                        state);
    __msg_Pub_Ptr->SaveLogInfo(info_level, info, info_code);
    LOG(INFO) << info;
    return true;
}

// 生成推行路径
bool GlobalPathPlanner::getImplementPath(v3_msgs::PathGenerateManagerRequest& req, v3_msgs::PathGenerateManagerResponse& res){
    LOG(INFO) << "getImplementPath start";
    res.response.success = true;
    res.response.message = "successful";

    bool thread_Busying = false;
    std::string info;
    int info_code = 0;
    std::string info_level = "INFO";

    __path_Width = ros::param::param<float>("/clean_width/path_width", 0.2);
    LOG(INFO) << "__path_Width from ros: " << __path_Width;

    // 预览示教路径
    if(req.is_preview){
        // 生成路径
        v3_msgs::TaskPath task_path;
        task_path.path_type.type = req.path_gen.path_types[0].type;
        task_path.poses = __implement_Planner_Ptr->GetImplementPath();
        if(task_path.poses.size() <= 0){
            info = "[GlobalPathPlanner::getImplementPath]Failed to preview implement recording!";
            info_level = "ERROR";
            info_code = res.response.error_code.error_code;
            res.response.message = info;
            res.response.error_code.error_msg = "Implement recording size is zero!";
        }else{
            if(req.control_mode.mode == v3_msgs::TaskControlMode::SWEEP_EDGE_MODE){
                res.edge_paths.emplace_back(task_path);
            }else{
                res.paths.emplace_back(task_path);
            }
            info = "Preview implement record [size:" + std::to_string(task_path.poses.size()) + "] successful!";
        }
    }else{
        // 开始记录
        if (req.path_gen.path_action.data == v3_msgs::PathGenerateAction::START_RECORD){
            if (__implement_Planner_Ptr->GetThreadBusing()){
                info = "[GlobalPathPlanner::getImplementPath]Implement recording thread is busying!";
            }
            else{
                __implement_Planner_Ptr->ClearPath();
                __implement_Planner_Ptr->SetThreadBusing(true);
                //开启多线程
                info = "[GlobalPathPlanner::getImplementPath]Initiating recording of the implement area!";
                std::thread t(&ImplementPlanner::RecordManualPath, __implement_Planner_Ptr);
                t.detach();
            }
        }
        // 取消记录
        if (req.path_gen.path_action.data == v3_msgs::PathGenerateAction::CANCEL_RECORD){
            info = "[GlobalPathPlanner::getImplementPath]Canceled recording of the implement area!";
            __implement_Planner_Ptr->SetThreadBusing(false);
        }
        // 保存记录
        if (req.path_gen.path_action.data == v3_msgs::PathGenerateAction::FINISHED_RECORD){
            if (!__implement_Planner_Ptr->GetThreadBusing()){
                res.response.success = false;
                res.response.error_code.error_code = 0x1501;
                info = "[GlobalPathPlanner::getImplementPath]Failed to save implement recording!";
                info_level = "ERROR";
                info_code = res.response.error_code.error_code;
                res.response.message = info;
                res.response.error_code.error_msg = "Reording thread is free!";
            }else{
                if(!req.is_preview)
                    __implement_Planner_Ptr->SetThreadBusing(false);
                // 生成路径
                v3_msgs::TaskPath task_path;
                task_path.path_type.type = req.path_gen.path_types[0].type;
                task_path.poses = __implement_Planner_Ptr->GetImplementPath();
                task_path.path_interval = __brush_Length;
                LOG(INFO) << "task_path.path_interval: " << task_path.path_interval;
                for(auto i = 0; i < task_path.poses.size(); i++){
                    if(i + 1 == task_path.poses.size()) break;
                        auto dx = task_path.poses[i].position.x -
                                task_path.poses[i + 1].position.x;
                        auto dy = task_path.poses[i].position.y -
                                task_path.poses[i + 1].position.y;
                        res.estimated_distance += std::sqrt(dx * dx + dy * dy);
                }
                // if(task_path.poses.size() > 0){
                    // auto path_pairs = __implement_Planner_Ptr->PoseTransferPairs(task_path.poses);
		            // std::vector<std::vector<std::pair<float, float>>> tmp_paths;
		            // tmp_paths.emplace_back(path_pairs);
                    // auto polys = __map_Proc_Ptr->ExtractPlanPathContours(tmp_paths, 0);
		            // for(auto poly : polys) res.task_area += __map_Proc_Ptr->CalcPlanArea(poly.second);
		            // res.task_area = res.task_area / 1.04;
                // }
                if(req.control_mode.mode == v3_msgs::TaskControlMode::SWEEP_EDGE_MODE){
                    res.edge_paths.emplace_back(task_path);
                }else{
                    res.paths.emplace_back(task_path);
                }
                res.estimated_speed = res.estimated_speed = Speed_Def::ObtainSpeed(__robotType, req.task_speed.speed);
                res.estimated_time = res.estimated_distance / res.estimated_speed;

                UpdateTaskStat(req, res, false);
                double resolution = __map_Info_Ptr->base_info.metadata.resolution;
                double clean_area = cv::countNonZero(__task_Stat.cv_map.MapData()) * resolution * resolution; // 待清洁区域面积 (跟任务报告保持一致)
                res.task_area = clean_area;
                LOG(INFO) << "res.task_area set to " << res.task_area;
                res.estimated_efficiency = (res.task_area / res.estimated_time) * 3600;

                info = "Saved implement record [size:" + std::to_string(task_path.poses.size()) + "] successful!";
            }
        }
    }
    __msg_Pub_Ptr->SaveLogInfo(info_level, info, info_code);
    LOG(INFO) << info;
    return true;
}

// 生成全覆盖路径
bool GlobalPathPlanner::getFullCoverPath(v3_msgs::PathGenerateManagerRequest& req, v3_msgs::PathGenerateManagerResponse& res){
    LOG(INFO) << "getFullCoverPath start";
    LOG(INFO) << "start: (" << req.path_gen.start.position.x << ", " << req.path_gen.start.position.y
              << "), offset: " << (int)req.path_gen.offset;
    res.response.success = true;
    res.response.message = "successful";

    // 日志消息属性
    double plan_time;
    std::string info;
    int info_code = 0;
    bool state = true;
    int points_sz = 0;
    std::string info_level = "INFO";

    __path_Width = ros::param::param<float>("/clean_width/path_width", 0.2);
    LOG(INFO) << "__path_Width from ros: " << __path_Width;

    // 将v3_msgs/PolygonData->geometry_msgs/Polygon
    std::vector<geometry_msgs::Polygon> all_polys;
    geometry_msgs::Polygon out_Poly;
    for(auto i = 0; i < req.path_gen.polygon.poses.size(); i++){
        geometry_msgs::Point32 pt;
        pt.x = req.path_gen.polygon.poses[i].position.x;
        pt.y = req.path_gen.polygon.poses[i].position.y;
        pt.z = req.path_gen.polygon.poses[i].position.z;
        out_Poly.points.emplace_back(pt);
    }
    all_polys.emplace_back(out_Poly); // all_polys: 任务区外轮廓

    // 不可达区域转为geometry_msgs/Polygon
    for(auto i = 0; i < req.path_gen.danger_areas.size(); i++){ // danger_areas 类似禁行区，已弃用
        geometry_msgs::Polygon inner_Poly;
        for(auto j = 0; j < req.path_gen.danger_areas[i].poses.size(); j++){
            geometry_msgs::Point32 pt;
            pt.x = req.path_gen.danger_areas[i].poses[j].position.x;
            pt.y = req.path_gen.danger_areas[i].poses[j].position.y;
            pt.z = req.path_gen.danger_areas[i].poses[j].position.z;
            inner_Poly.points.emplace_back(pt);
        }
        all_polys.emplace_back(inner_Poly);
    }

    // 解析生成路径类型
    bool region_inflate = true; // 手绘区域需要腐蚀半车宽, 示教区域不需要
    int16_t gen_path_type = GetPathType(req.path_gen.path_types);
    if(gen_path_type == FullCoverPlanner::E_IMPLEMENT){
        gen_path_type = GetPathType(req.path_gen.path_types);
        region_inflate = false;
    }

    // 获取默认充电桩
    std::pair<bool, std::tuple<float, float, float>> docker_pose;
    // std::pair<bool, std::tuple<float, float, float>> docker_pose = getDefaultDockerPose();
    if(docker_pose.first){
        LOG(INFO) << "docker pose (" << std::get<0>(docker_pose.second) << ", " << std::get<1>(docker_pose.second) << ")";
    }

    // 调用接口
    ros::Time t0 = ros::Time::now();
    // __full_Cover_Planner_Ptr->UpdateParam(__map_Info_Ptr->base_info.name, __map_Dir,
    //                                     __map_Info_Ptr->base_info.metadata.origin.position.x,
    //                                     __map_Info_Ptr->base_info.metadata.origin.position.y,
    //                                     __map_Info_Ptr->base_info.metadata.resolution,
    //                                     __map_Info_Ptr->base_info.area, __path_Width, req.option);
    __full_Cover_Planner_Ptr->UpdateParam("test", __map_Dir,
                                        __map_Grid_Ptr->info.origin.position.x,
                                        __map_Grid_Ptr->info.origin.position.y,
                                        __map_Grid_Ptr->info.resolution,
                                        0.0, __path_Width, req.option);
    auto res_paths = __full_Cover_Planner_Ptr->SolvedPath(req.path_gen.offset, gen_path_type, // req.path_gen.offset: 动态路径偏移量, unit: 栅格
                                                        req.path_gen.start, region_inflate,
                                                        all_polys, docker_pose,
                                                        res.task_area, info);
    plan_time = (ros::Time::now() - t0).toSec();
    if(req.option == 0 && res_paths.size() == 0){
        res.response.success = false;
        res.response.error_code.error_code = 0x1503;
        res.response.message = "[GlobalPathPlanner::getFullCoverPath]Failed to generate full cover path!";
        res.response.error_code.error_msg = info;
        info = "[GlobalPathPlanner::getFullCoverPath]" + res.response.error_code.error_msg;
        info_code = res.response.error_code.error_code;
        info_level = "ERROR";
        state = false;
        points_sz = 0;
    }else{
        for(auto res_path : res_paths){
            for (auto it = res_path.second.begin(); it != res_path.second.end(); it++)
            {
                v3_msgs::TaskPath task_path;
                for(auto iter = it->begin(); iter != it->end(); iter++){
                    geometry_msgs::Pose pt;
                    pt.position.x = iter->first;
                    pt.position.y = iter->second;
                    if(iter + 1 != it->end()){
                        auto dx = iter->first - (iter + 1)->first;
                        auto dy = iter->second - (iter + 1)->second;
                        res.estimated_distance += std::sqrt(dx * dx + dy * dy);
                    }
                    task_path.poses.push_back(pt);
                    points_sz++;
                }
                task_path.path_type.type = gen_path_type;
                if(gen_path_type == FullCoverPlanner::E_EDGE) 
                    task_path.path_interval = __brush_Length;
                else task_path.path_interval = __path_Width;
                LOG(INFO) << "task_path.path_interval: " << task_path.path_interval;
                if(res_path.first == 1){
                    res.edge_paths.emplace_back(task_path); // 贴边绕障路径
                }else if(res_path.first == 0){
                   res.paths.emplace_back(task_path); // 覆盖清洁路径
                }
                res.estimated_speed = Speed_Def::ObtainSpeed(__robotType, req.task_speed.speed);
                res.estimated_time = res.estimated_distance / res.estimated_speed; // TODO: rj.wang
            }
        }

        UpdateTaskStat(req, res, true);
        double resolution = __map_Grid_Ptr->info.resolution;
        double clean_area = cv::countNonZero(__task_Stat.cv_map.MapData()) * resolution * resolution; // 待清洁区域面积 (跟任务报告保持一致)
        res.task_area = clean_area;
        LOG(INFO) << "res.task_area set to " << res.task_area;
        res.estimated_efficiency = (res.task_area / res.estimated_time) * 3600;

        info = "[GlobalPathPlanner::getFullCoverPath]Generate full cover path successful!";
    }
    info = __msg_Pub_Ptr->FCPPNormalLogInfo(all_polys.front(),
                                            plan_time, info,
                                            points_sz, state);
    __msg_Pub_Ptr->SaveLogInfo(info_level, info, info_code);
    LOG(INFO) << info;
    return true;
}

// 获取路径类型
int16_t GlobalPathPlanner::GetPathType(std::vector<v3_msgs::PathGenerateType> & path_types){
    int16_t clean_mode = -1;
    if(path_types.size() == 0) clean_mode = FullCoverPlanner::E_BOUSTROPHEDON;
    else{
        if(path_types.size() == 1){
            if(path_types[0].type == v3_msgs::PathGenerateType::BOUSTROPHEDON_METHOD)
                clean_mode = FullCoverPlanner::E_BOUSTROPHEDON;
            else if(path_types[0].type == v3_msgs::PathGenerateType::WRAPPER_METHOD)
                clean_mode = FullCoverPlanner::E_LOOP;
            else if(path_types[0].type == v3_msgs::PathGenerateType::EDGE_METHOD)
                clean_mode = FullCoverPlanner::E_EDGE;
            else if(path_types[0].type == v3_msgs::PathGenerateType::MANUAL_POLYGON_METHOD)
                clean_mode = FullCoverPlanner::E_IMPLEMENT;
            else
                clean_mode = FullCoverPlanner::E_AUTO;
        }else if(path_types.size() == 2){
            if((path_types[0].type == v3_msgs::PathGenerateType::EDGE_METHOD &&
            path_types[1].type == v3_msgs::PathGenerateType::BOUSTROPHEDON_METHOD) ||
            (path_types[0].type == v3_msgs::PathGenerateType::BOUSTROPHEDON_METHOD &&
            path_types[1].type == v3_msgs::PathGenerateType::EDGE_METHOD)){
                clean_mode = FullCoverPlanner::E_EDGE_BOUSTROPHEDON;
            }else if((path_types[0].type == v3_msgs::PathGenerateType::EDGE_METHOD &&
                    path_types[1].type == v3_msgs::PathGenerateType::WRAPPER_METHOD) ||
                    (path_types[0].type == v3_msgs::PathGenerateType::WRAPPER_METHOD &&
                    path_types[1].type == v3_msgs::PathGenerateType::EDGE_METHOD)){
                clean_mode = FullCoverPlanner::E_EDGE_LOOP;
            }else{
                clean_mode = FullCoverPlanner::E_EDGE_BOUSTROPHEDON;
            }
        }else{
            clean_mode = FullCoverPlanner::E_AUTO;
        }
    }
    return clean_mode;
}

// 获取默认充电桩位置
std::pair<bool, std::tuple<float, float, float>> GlobalPathPlanner::getDefaultDockerPose()
{
    bool find_result = false;
    std::tuple<float, float, float> pose;
    try
    {
        v3_msgs::MapInfoRequest map_info_req;
        map_info_req.request.map_info.base_info = __map_Info_Ptr->base_info;
        map_info_req.request.option = map_info_req.request.QUERY_MAP_INFO;
        __map_Info_Client.call(map_info_req);
        if (map_info_req.response.response.success)
        {
            int size = map_info_req.response.map_info.marker_point_list.size();
            for (int i = 0; i < size; i++)
            {
                auto &marker_one = map_info_req.response.map_info.marker_point_list[i];
                // 默认充电桩（普通或梯控）
                if ((marker_one.type == marker_one.CHARGING_PILE ||
                    marker_one.type == marker_one.CHARGING_PILE_ELEVATOR) &&
                    marker_one.is_default == true)
                {
                    auto tmp_pose = PoseConvertTuple(marker_one.pose);
                    pose = Docker2LandingPtn(tmp_pose, __landing_Distance,
                                            Docking_Def::ObtainDockingDirection(__robotType));
                    find_result = true;
                    break;
                }
            }
        }
    }
    catch (...)
    {
        find_result = false;
    }
    return std::make_pair(find_result, pose);
}

// 将geometry_msgs::Pose转换到tuple
std::tuple<float, float, float> GlobalPathPlanner::PoseConvertTuple(const geometry_msgs::Pose &pose)
{
    tf::Quaternion quat;
    tf::quaternionMsgToTF(pose.orientation, quat);
    double roll, pitch, yaw;                      // 定义存储r\p\y的容器
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw); // 进行转换
    return std::make_tuple(pose.position.x, pose.position.y, yaw);
}

// 将充电桩的位置转化为滑行点的位置
std::tuple<float, float, float> GlobalPathPlanner::Docker2LandingPtn(std::tuple<float, float, float> &docker_pose,
                                                                    float& land_dist, int backward)
{
    std::tuple<float, float, float> landing_pose;
    double theta = std::get<2>(docker_pose);
    if(backward == 1)
    {
       theta = theta + M_PI;
    }
    auto x = std::get<0>(docker_pose) + land_dist * cos(theta);
    auto y = std::get<1>(docker_pose) + land_dist * sin(theta);
    landing_pose = std::make_tuple(x, y, theta);
    return landing_pose;
}

// 更新__task_Stat
void GlobalPathPlanner::UpdateTaskStat(
    const v3_msgs::PathGenerateManagerRequest& req,
    const v3_msgs::PathGenerateManagerResponse& res, const bool area_clean) {
    // auto& metadata = __map_Info_Ptr->base_info.metadata;
    auto& metadata = __map_Grid_Ptr->info;
    __task_Stat.Init(common::statistics::ToBoundaryType(req.path_gen.path_types, area_clean),
                     common::statistics::ToBoundary(req.path_gen.polygon.poses, area_clean),
                     common::statistics::ToCleanPaths(res.edge_paths, res.paths),
                     __path_Width,
                     Pose(metadata.origin.position.x + 0.5 * metadata.resolution, metadata.origin.position.y + 0.5 * metadata.resolution),
                     metadata.resolution,
                     metadata.width,
                     metadata.height);
    __task_Stat.CalcGridsToClean(true, 5);
}

int main(int argc, char** argv){
    ros::init(argc, argv, "global_path_planner");
    common::log::InitLog("gpp", common::log::kGlobalPathPlannerLogPath);

    GlobalPathPlanner __global_Path_Planner;

    // ROS_INFO中文显示
    setlocale(LC_ALL, "");

    ros::spin();
    common::log::ShutdownLog();
    return (0);
}